# coding=utf-8
import numpy as np
import matplotlib
import matplotlib.pyplot as plt
import csv
import os
import math
# 获取当前脚本所在位置 v_y,yaw_rate,e_y,e_heading,tire_f_angle
print(os.path.dirname(os.path.realpath(__file__)))
save_path = os.path.dirname(os.path.realpath(__file__))+"/../paper3_figure/"
data_path = os.path.dirname(os.path.realpath(__file__))+"/../paper3data/"


def brush_model_calc(Fz, alpha, miu, c_alpha):
    bool_var = math.atan(3.0*miu*Fz/c_alpha)
    tan_alpha = math.tan(alpha)
    if math.fabs(alpha) < bool_var:
        return -c_alpha*tan_alpha + math.pow(c_alpha, 2.0)/3/miu/Fz*math.fabs(tan_alpha)*tan_alpha-math.pow(c_alpha, 3.0)/27/math.pow(miu, 2.0)/Fz/Fz*math.pow(tan_alpha, 3.0)
    elif alpha > 0:
        return -miu*Fz
    else:
        return miu*Fz


def derivate_brush_model(Fz, alpha, miu, c_alpha):
    tan_alpha = math.tan(alpha)
    deri_tan_alpha = 1.0/math.pow(math.cos(alpha), 2.0)
    bool_var = math.atan(3.0*miu*Fz/c_alpha)
    if math.fabs(alpha) < bool_var:
        if alpha > 0:
            temp_var = -c_alpha+2.0*c_alpha*c_alpha/3.0/miu/Fz*tan_alpha - \
                math.pow(c_alpha, 3.0)/9.0/math.pow(miu, 2.0) / \
                Fz/Fz*math.pow(tan_alpha, 2.0)
        else:
            temp_var = -c_alpha-2.0*c_alpha*c_alpha/3.0/miu/Fz*tan_alpha - \
                math.pow(c_alpha, 3.0)/9.0/math.pow(miu, 2.0) / \
                Fz/Fz*math.pow(tan_alpha, 2.0)
    else:
        temp_var = 0.0
    return temp_var*deri_tan_alpha


Fz = 4800
alpha_np = np.arange(-25.0, 25.0, 1.0) * math.pi / 180.0

fy_list = []
deri_fy_list = []
for alpha in alpha_np:
    fy_list.append(brush_model_calc(Fz, alpha, 1.0, 96000))
    deri_fy_list.append(derivate_brush_model(Fz, alpha, 1.0, 96000))
plt.plot(alpha_np*180.0/math.pi, fy_list)


plt.show()
plt.plot(alpha_np*180.0/math.pi, deri_fy_list)
plt.show()
print(brush_model_calc(Fz, 0.1, 1.0, 96000))
